COMMENT ⊗   VALID 00008 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	.NEWSEC PROGRAMMING EXAMPLES,BOLTING A BRACKET
C00007 00003	.ex1: NEWSSS EXAMPLE ONE
C00016 00004	.NEWSSS EXAMPLE TWO
C00026 00005	.NEWSSS EXAMPLE THREE
C00040 00006
C00044 00007	.NEWSS EXAMPLES OF COORDINATED ACTION, COORDINATED ACTION
C00052 00008	.NEWSS A α`VERY HIGH LEVELα' EXAMPLE
C00058 ENDMK
C⊗;
.NEWSEC PROGRAMMING EXAMPLES,BOLTING A BRACKET
.NEWSS BOLTING A BRACKET ONTO A BEAM, BOLTING A BRACKET

.FILL

This  is intended  to  be  a  series of  progressively  more  complex
examples  which demonstrate  some of the  features in  AL, including
affixment, control structures,  macros, and library routines.
The first set of
the examples have  essentially the same goal: bolt a  bracket to a beam.
Each  example takes  into  account more  possibilities or  contains a
different way of expressing the same thing.

The initial affixment structure is:
.NOFILL

        	STATION
		    YELLOW
		    BLUE
		 BRACKET
		    BRACKET_HOLE
		    BRACKET_GRASP
		 BOLT
		 BEAM
		    BEAM_HOLE

The final affixment structure is:

		STATION
		    YELLOW
		    BLUE
		BEAM
		    BEAM_HOLE
		    BRACKET
			BRACKET_HOLE
			BRACKET_GRASP
		    BOLT

.FILL

The initial structure  can be created by the  following declarations
and assignments.
See {NEWFIG Initial World,FULL}.
.NOFILL


FRAME beam, beam_hole;
FRAME bracket, bracket_hole, bracket_grasp;
FRAME bolt;

beam α← FRAME(ROT(Z, 90*DEG), VECTOR(30, 24.2, 0));
.COMT 4
   α{Beam is not affixed to anything initially.  Thus its
	default DEPROACH is the station's DEPROACH which is:
	TRANS(NILROT, 10*CM*Z)α}
.END

beam_hole α← beam * FRAME(ROT(X, -90*DEG),  VECTOR(5.1, 0, 15));
.COMT  4
	α{FRAME(ROT(X, -90*DEG),  VECTOR(5.1, 0, 15))
	 is the relative transform from
	beam to the beam_hole.  Another way of looking at this is that
	within the beam's frame of reference, the beam_hole is at
	FRAME(ROT(X, -90*DEG),  VECTOR(5.1, 0, 15))
	The premultiplication by beam transforms
	this relative location out to the corresponding position (in station 
	coordinates) with respect to the current location of beam.α}
.END


AFFIX beam_hole TO beam;
ASSERT FORM(DEPROACH, beam_hole, TRANS(NILROT, VECTOR(0, 0, -3));
.COMT  4
	α{this sets up a DEPROACH of -3 centimeters in the Z direction
	of the beam_hole's coordinate system.α}
.END

bracket α← FRAME(ROT(Z,90*DEG),  VECTOR(20, 40, 0));
bracket_hole α← bracket * FRAME(ROT(X, 180*DEG),  VECTOR(5.1, 2, 0));
AFFIX bracket_hole TO bracket;
bracket_grasp α← bracket * FRAME(ROT(X, 180*DEG), VECTOR(0, 1.5, 5));

AFFIX bracket_grasp TO bracket RIGIDLY;
.COMT  4
	α{Notice that changing bracket_grasp will automatically change
	bracket, which in turn will automatically change bracket_hole.  This
	is very handy if the position of the whole `object' is being
	updated by one grasping position (ie. bracket_grasp.α}
.END
bolt α← FRAME(ROT(Z,90*DEG)*ROT(X,180*DEG), VECTOR(30, 60, 5));
.COMT 4
   α{The bolt is assumed to be sticking out of a dispenser.α}
.END
.ex1: NEWSSS EXAMPLE ONE

The task involves the following steps:
.BEGIN INDENT 8,12; FILL;PREFACE 0;

	(1) Pick up the bracket with the YELLOW arm and position
	    it next to the beam so that the holes line up.

	(2) Pick up the bolt with the BLUE arm and insert
	    it in the hole (in this example it is not screwed
	    in; a later example will use a socket driver
	    to tighten the bolt).

	(3) Return both arms to park.
.END

.FILL
The bracket is assumed to be 1 cm thick , and the bolt 4 cm long.
The following  program  is a  straightforward way  to express the
motions and feedback  necessary to carry out the task.  Everything is
assumed to  be in  the right  place and  every motion  is assumed  to
accomplish its  desired effect.  For example,   this  program assumes
that  the arm  is  accurate enough  to align  the bracket_hole  with the
beam_hole and to insert the bolt without hitting the side or binding.
Later examples will take this type of error into account.
.NOFILL

DEFINE OZ = "72.007789*DYNES";
.comt 4
	α{This macro defines a unit of force OZ equal to 1/16 poundalα}
.END

OPERATE YFINGERS WITH OPENING=3*CM;
MOVE YELLOW TO bracket_grasp;
.comt 4
	α{Since bracket_grasp does not have a DEPROACH explicitly
	associated with it, the compiler checks to see if it is 
	affixed to anything.  It is: bracket.  But bracket does not
	have a DEPROACH associated with it either.  Is it affixed
	to anything?  No.  Therefore, by default the compiler uses the STATION's
	DEPROACH (ie. TRANS(NILROT,10*CM*Z)) as the approach for
	bracket_grasp.α}
.END

CENTER YELLOW;
.COMT 4
	α{This closes the fingers until they grab something.α}
.END
bracket_grasp α← YELLOW;
.COMT 4
	α{Since bracket_grasp is RIGIDLY affixed to bracket, this
	statement updates bracket and hence anything affixed to
	bracket (eg. bracket_hole).  In effect, the assumption being
	made is that the position of the whole `object' (ie. the bracket)
	can be updated by locating bracket_grasp.  In the usage above the
	arm moves to the planning position for bracket_grasp and then
	centers itself about the object between its fingers.  Notice that
	the final position of the arm may very well not be bracket_grasp
	(because of the accommodation during the centering).  Therefore,
	the bracket might not be where it was planned to be.  This discrepancy
	between the planned world and the `actual' world has to be
	reconciled.  The simplest assumption (and the assumption being used here)
	is that the only difference between the planned location and the
	actual is that the `whole' bracket has been moved along the line between
	the fingers so that bracket_grasp is where the arm found it.  More
	complicated updating could be done by visually locating the bracket
	and reseting bracket or by feeling the bracket two or three times,
	combining the resulting locations into a new estimate of bracket's
	location, and reseting bracket.  Notice that if the CENTER moved the arm
	away from the planned location and no updating were done, the AFFIX
	statement which follows  would affix the bracket to
	the YELLOW arm in such a way that the bracket was assumed to be at
	its planning position (which would be wrong).  The subsequent move
	to the beam_hole would also be off by the same amount.α}
.END

AFFIX bracket TO YELLOW;
MOVE bracket_hole TO beam_hole;
.COMT 4
	α{Notice that the bracket approaches the beam from the side
	(not from above) because of the DEPROACH set up for beam_hole.
	In this example the bracket is assumed to go right next
	to the beam.
	This MOVE is a move for the YELLOW arm (because the bracket
	is AFFIXed to it).  From the definition of affixment this
	means that anything affixed to the YELLOW arm is automatically
	moved.  Thus, bracket, bracket_hole, and bracket_grasp are all
	updated.  The fact that the move was specified by mentioning
	bracket_hole (and not YELLOW) does not change the automatic
	updating within the graph structure.  Notice, in particular,
	that this is quite different from:
.BEGIN NOFILL
		AFFIX bracket TO YELLOW
		bracket_hole α← beam_hole
.END
	which would change the value of bracket_hole and the relative
	position between bracket and bracket_hole, but leave YELLOW and bracket
	unchanged.α}
.END

OPERATE BFINGERS WITH OPENING=3*CM;
MOVE BLUE TO bolt;
.COMT 4
	α{The station's APPROACH is used since the bolt is not affixed
	to anything.α}
.END

CENTER BLUE;
bolt α← BLUE;
.COMT 4
	α{This insures that the latest value of bolt is used in the AFFIX
	command below.α}
.END

AFFIX bolt TO BLUE;
MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole;
.COMT 4
	α{This should position the bolt .3 centimeters from the bracket.
	That is, the YELLOW arm is now holding the bracket right next to the
	beam (with the bracket_hole aligned with the beam_hole)  and the BLUE
	arm is holding the bolt 5.3 centimeters away from the bracket_hole
	(which is equivalent to beam_hole).  But remember that the
	bracket is 1 cm thick and the bolt is 4 cm long;
	thus the tip of the bolt is 1.3 cm from the beam_hole
	(or .3 off of the bracket).α}
.END

MOVE BLUE TO ⊗ + VECTOR(0, 0, 5) WRT beam_hole;
    WITH FORCE = 0 ALONG X,Y OF BLUE
    ON FORCE(Z WRT BLUE) > 60*OZ DO STOP BLUE;
.COMT 4
	α{The arm stops when the bolt hits the bottom of the hole.
	No DEPARTURE or APPROACH is used because the destination
	involves the "⊗" construct.α}
.END

OPERATE YFINGERS WITH OPENING = 3*CM;
UNFIX bracket FROM YELLOW;
AFFIX bracket TO beam;
MOVE YELLOW TO YPARK;

OPERATE BFINGERS WITH OPENING = 3*CM;
UNFIX bolt FROM BLUE;
AFFIX bolt TO beam;
MOVE BLUE TO BPARK;

WRITE("Finished");
END;

.NEWSSS EXAMPLE TWO

.FILL

This version adds a number of checks  (and some automatic recoveries)
for possible run-time errors such as not inserting the bolt.  It also
utilizes the  COBEGIN  - COEND  capability to  describe  simultaneous
(unordered,   independent) actions.   Thus,   the  Yellow arm  can be
picking  up the bracket  and positioning  it near the  beam while the
Blue arm is  picking up the bolt.   Collision avoidance is  currently
the responsibility of the user.


.NOFILL

DEFINE OZ="((72.007789*GM*CM)/(SEC*SEC))";

positioning: COBEGIN
    ypickup: BEGIN α{%4pick up bracket by YELLOW%*α}
    OPERATE YFINGERS WITH OPENING=3*CM;
    MOVE YELLOW	TO bracket_grasp;
    CENTER YELLOW
        ON OPENING = 0*CM DO 
	    missed: BEGIN α{%4missed bracket%*α}
	    STOP YELLOW;
	    SCALAR flag;
	    OPERATE YFINGERS WITH OPENING=3*CM;
	    MOVE YELLOW 
		TO bracket_grasp * DEPROACH(bracket_grasp) DIRECTLY;
.COMT 14
                α{This should safely move the arm away so the
		    operator can easily insert the missing bracket.  It moves
		    the arm back out to the bracket_grasp's approach point
		    at runtime.α}

.END
	    WRITE("The bracket is missing.  Position it and type `1' to try again");
	    READ(flag);
	    IF flag ≠ 1 THEN ABORT ("Giving up; you didn't type `1'");
.COMT 14
		α{The ABORT stops everything, saves the world, and forces
		    the operator to deal with the problem at supervisor level,
		    possibly investigating the saved information, reinitializing
		    the world to some previous state and restarting.α}
.END
	    MOVE YELLOW TO bracket_grasp DIRECTLY;
.COMT 14
	α{this results in a simple move without a DEPARTURE
		    or an APPROACH.α}
.END
	    CENTER YELLOW
		ON OPENING=0*CM DO ABORT("I tried twice; I give up!");
	    END missed;

    YELLOW α←α← bracket_grasp;
.COMT 8
	α{This tells the compiler that the yellow arm can be
	    assumed to be at bracket_grasp no matter how control got here,
	    eg. possibly moving away and retrying the grasp.
	    The "α←α←"
	    specifies that the planning value of bracket_grasp
	    should be used to update the compiler's view of where the YELLOW
	    is.α}
.END
    bracket_grasp α← YELLOW;
.COMT 8
	α{This generates code to be run at run-time which updates
	    the frame bracket_grasp (which in turn updates bracket and bracket_hole).
	    The result is that the following AFFIX uses the best run-time
	    value of the bracket's position.α}
.END
    AFFIX bracket TO YELLOW;
    MOVE bracket_hole TO beam_hole + VECTOR(0, 0, 1.3) WRT beam_hole;
.COMT 8
	α{This uses the STATION's DEPARTURE (since the bracket is not
	    affixed to anything) and the beam_hole's approach
	    (since it is the only frame mentioned in the destination).
	    This move should position the bracket just off of
	    the beam.  The next motion pushes it up against the beam.α}
.END
    MOVE YELLOW TO ⊗ + VECTOR(0, 0, .5) WRT beam_hole
	ON FORCE(Z WRT beam_hole) > 50*OZ DO STOP YELLOW
	ON ARRIVAL DO ABORT ("I seem to have gone too far");
.COMT 8
	α{Give up if the expected force is not felt.
		"ARRIVAL" means that the arm reached its destination
		without being stopped by any of the
		condition monitors.  In this case this means that the arm
		did not reach the expected force, which means that
		something went wrong.
		The STOP YELLOW disables all condition monitors for the yellow
		arm.α}
.END
    END ypickup;

    bpickup: BEGIN α{pick up bolt by BLUEα}
.COMT 8
   α{Meanwhile the BLUE arm can be picking up the bolt.α}
.END
    OPERATE BFINGERS WITH OPENING=3*CM;
    MOVE BLUE TO bolt;
    CENTER BLUE;  
.COMT 8
	α{Assume everything is OK.α}
.END
    bolt α← BLUE;
    AFFIX bolt TO STATION;
    END bpickup
COEND positioning;

.COMT 0
α{The bracket should be positioned next to the beam and the BLUE arm
	should be holding the bolt.α}
.END

MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole
    WITH DEPROACH(beam_hole);
.COMT 4
   α{This should position the bolt .3 centimeter off of the bracket.α}
.END

.COMT 0
	α{Now begin a search just in case the bolt doesn't immediately go in the
	hole: make .2 cm steps around in a spiral; if the bolt does not
	go in within nine tries, abort the program.α}
.END
FRAME set; SCALAR n; 
.COMT 4
   α{n is the number of attempts.α}
.END
n α← 0;
set α← BLUE;  
.COMT 4
   α{Save initial arm position.α}
.END
SEARCH BLUE
    INCREMENT .2*CM
    ACROSS PLANE(NILVEC,Z WRT beam_hole)
    REPEATING
	inserting: BEGIN 
	MOVE BLUE TO ⊗ + VECTOR(0, 0, 1.6) WRT beam_hole
	    ON FORCE(Z WRT beam_hole) > 60*OZ DO
		missed: BEGIN
		STOP BLUE;
		n α← n + 1;
		IF n > 9 THEN ABORT ("Giving up the search");
		MOVE BLUE TO set
		END missed
	    ON ARRIVAL DO TERMINATE;
.COMT 14
                   α{This means that if the MOVE succeeds in reaching
			its goal, stop the search.  TERMINATE is a key word
			within SEARCHs.α}
.END
	END inserting;

BLUE α←α← beam_hole + VECTOR(0, 0, 3.7) WRT beam_hole;
.COMT 4
   α{Expect to have the bolt (which is 4 cm long) .3 cm into the hole.α}
.END

MOVE BLUE TO ⊗ * FRAME(ROT(Z, 90*DEG), VECTOR(0, 0, 4))
    WITH FORCE = 0 ALONG X,Y OF BLUE
    ON FORCE(Z WRT BLUE) > 60*OZ DO STOP BLUE;
.COMT 8
           α{This moves the arm 4 cm straight ahead and
		twists it 90 degrees about its Z axis (ie. straight
		ahead).  Thus it moves ahead and twists.α}
.END

disengage: COBEGIN
    foryellow: BEGIN 
    OPERATE YFINGERS WITH OPENING = 3*CM;
    UNFIX bracket FROM YELLOW;
    AFFIX bracket TO beam;
    MOVE YELLOW TO YPARK
    END foryellow;

    forblue: BEGIN
    OPERATE BFINGERS WITH OPENING = 3*CM;
    UNFIX bolt FROM BLUE;
    AFFIX bolt TO beam;
    MOVE BLUE TO BPARK
    END forblue
COEND disengage;
WRITE("Finished");
END;

.NEWSSS EXAMPLE THREE

.FILL
This example employs a  text macro to simplify definitions,   a macro
to  shorten the code for  searching,  and a  library routine to grasp
things.   The  library  routine is  supposed  to cover  a  number  of
possibilities and provide for a  number of parameters.  Since library
routines  can be called with a subset  of their parameters filled in,
the routine's flexibility is not oppressive for those  users who just
want to do  something simple.

.NOFILL

DEFINE define_wrt(new_frame, main_frame, position) =
	"new_frame α← main_frame * position;
	 AFFIX new_frame TO main_frame";

A typical call might be:

	define_wrt(bracket_hole, bracket, FRAME(ROT(X, 180*DEG), VECTOR(5.1, 2, 0));

which would expand into:

	bracket_hole α← bracket * FRAME(ROT(X, 180*DEG), VECTOR(5.1, 2, 0));
	AFFIX bracket_hole TO bracket;


.FILL
The following macro produces a string of tokens which imply a compile-time
check on  the value of the conditional expanded by the parameter
RIGID.  If RIGID evaluates to TRUE then the token sequence which rigidly
affixes the new frame to the main frame is used.
.NOFILL

DEFINE DEFINE_WRT(new_frame, main_frame, position, rigid) =
   "new_frame α← main_frame * position;
    PLAN IF rigid 
	THEN AFFIX new_frame TO main_frame RIGIDLY
	ELSE AFFIX new_frame TO main_frame";



Another, more complicated macro to facilitate a normal search:

DEFINE normal_search(the_arm, increm, dist_fwd, stopping_force, num_tries) =
	"BEGIN  α{%4This BEGIN is part of the macro code.%*α}
	 FRAME set; SCALAR n; 
.COMT 10
	   α{n is the number of attempts.α}
.END
	n α← 0;
	set α← the_arm;  
.COMT 10
	   α{Save initial arm position.α}
.END
	SEARCH the_arm
	    INCREMENT increm
	    ACROSS PLANE(NILVEC, Z WRT the_arm)
	    REPEATING
		insertion: BEGIN
		MOVE the_arm TO ⊗ + (dist_fwd*Z) WRT the_arm
		    ON FORCE(Z WRT the_arm) > stopping_force DO
		        missed: BEGIN
			STOP the_arm;
			n α← n + 1;
			IF n > num_tries THEN ABORT(""Giving up"");
			MOVE the_arm TO set;
			END missed
		    ON ARRIVAL DO TERMINATE
		END insertion;
	ASSERT the_arm = α#(set) +  VECTOR(0, 0, dist_fwd);
.COMT 10
	α{This changes the compiler's view to believe that the arm
		succeeds on the first attempt, and hence the planning
		value for the arm will be the distance forward plus set.α}
.END
    END";

.FILL
Notice that a pair of adjacent quotes inside of a macro definition
(delimited by quotes) denotes a single  quote.

A typical call would be:
.NOFILL
	normal_search(YELLOW, .2*CM, 1.6*CM, 60*OZ, 9);



The above macro could easily be made into a library routine as follows:

ROUTINE normal_search(FRAME the_arm; DISTANCE SCALAR increm, dist_fwd;
			FORCE SCALAR stopping_force;
			SCALAR num_tries(DEFAULT 9));
    BEGIN
    :
    END;

The corresponding call:

	normal_search(YELLOW, .2*CM, 1.6*CM, 60*OZ, 9);

.FILL
The "9" is a default value if no value is specified in the call.  Thus, by
naming the parameters the same call can be made by:
.NOFILL

	normal_search(the_arm=YELLOW, dist_fwd=1.6*CM,
	    stopping_force=60*OZ, increm=.2*CM);

Notice that the order is not important if the parameters are named.




.FILL
The following routine is a library routine to grasp things.  Basically it
does the following:
.BEGIN FILL; INDENT 8,12
	(1) Optionally open to an opening_before_departure.

	(2) Depart via a departure (if there is one; a special_departure
	    can be specified).

	(3) Start opening the fingers to the opening_for_approach at the 
	    departure point (if special_departure is specified, use it.
	    Otherwise, use the standard DEPROACH value.).

	(4) Approach the grasping_point via the APPROACH (if a
	    special_approach is specified, use it).

	(5) Center on the object. (If the fingers close so that the opening is
	    less than (thickness#-#.10) call the operator and give him one
	    chance to re-position the object and try again.)

	(6) Upon successfully centering on the grasp_point, update the
	    object's position by assigning the grasp_point the current hand
	    location  (this, of course, assumes that either the grasp_point
	    and the object are the same frame or that the grasp_point is
	    RIGIDLY affixed to the object).
.END

.NOFILL
Notice that this routine can be used by either arm.

ROUTINE grasp(TRANS special_departure, special_approach;
	      FRAME ATOM the_arm (DEFAULT YELLOW);
	      FRAME object, grasp_point, thing_object_affixed_to;
	      DISTANCE SCALAR opening_before_departure,
		  opening_for_approach(DEFAULT 15*CM),
		  thickness(DEFAULT .3*CM));
.COMT 8
	   α{Special_departure is a trans for the relative
		position of departure.
	    Special_approach is a trans for the relative
		position of the approach.
	    Thing_object_affixed_to is the name of the frame that the object
		is affixed to (if there is one) before the grasp
		routine is called.  It is used to specify from what the
		object should be unfixed upon being grasped.
	    Thickness is defaulted to .3*CM so that the condition monitor
		ON#OPENING#<#(thickness#-#.2*CM)#DO ... will do a
		reasonable thing.α}
.END

    grasping: BEGIN 
    ATOM the_fingers;
    CLAUSE t, u;
    PLAN IF α#(the_arm) = BLUE
	THEN the_fingers α←α← BFINGERS
	ELSE the_fingers α←α← YFINGERS;
.COMT 8
	α{This sets up the atom the_fingers
	    to expand into the correct device name for the
	    OPERATE statements (depending upon the choice of arm).α}
.END
    PLAN IF SPECIFIED(opening_before_departure) THEN
	OPERATE α#(the_fingers) WITH OPENING=opening_before_departure;

.COMT 8
	α{The next statement sets up a clause,
	u, which contains the phrase "WITH#APPROACH#=#<the special>"
	or NILDEPROACH depending upon whether or not a special approach
	has been specified.  This constructed phrase is used in two
	or three places below to insure that the desired approach is
	being used.α}
.END
    PLAN IF SPECIFIED(special_approach) 
	THEN u α←α← CLAUSE(WITH APPROACH = special_approach )
	ELSE u α←α← NILCLAUSE;
    PLAN IF SPECIFIED(special_departure)
	THEN MOVE α#(the_arm) TO grasp_point
		WITH DEPARTURE=NILDEPROACH
		VIA α#(the_arm) * special_departure THEN
		    BEGIN
		    OPERATE α#(the_fingers)
			WITH OPENING=opening_for_approach
		    END
		α#(u)
	ELSE MOVE α#(the_arm) TO grasp_point
		WITH DEPARTURE=NILDEPROACH
		VIA α#(the_arm) * DEPROACH(grasp_point) THEN
		    BEGIN
		    OPERATE α#(the_fingers)
			WITH OPENING=opening_for_approach
		    END
		α#(u);

    CENTER α#(the_arm)
	ON OPENING < (THICKNESS-.2*CM) DO
	    missed: BEGIN 
	    STOP α#(the_arm);
	    SCALAR flag;
	    OPERATE the_fingers WITH OPENING=opening_for_approach;
	    PLAN IF SPECIFIED(special_approach)
		THEN BEGIN α{%4move to special approach point%*α}
			MOVE α#(the_arm) TO α#(the_arm) * special_approach
				DIRECTLY
		     END 
		ELSE BEGIN α{%4use the normal approach%*α}
			MOVE α#(the_arm)
			    TO α#(the_arm)* DEPROACH(grasp_point)
			    DIRECTLY;
		     END 
	    WRITE("Grasp failed; Type a `1' to retry");
	    READ(flag);
.COMT 12
	       α{This is simply "wait for proceed".α}
.END
	    IF flag ≠ 1 THEN ABORT;
	    MOVE α#(the_arm) TO grasp_point DIRECTLY;
	    CENTER α#(the_arm)
		ON OPENING < (THICKNESS-.2*CM) DO ABORT ("Closed on air");
	    END missed;

    grasp_point α← α#(the_arm);
    PLAN IF SPECIFIED(thing_object_affixed_to) THEN
	UNFIX object FROM thing_object_affixed_to;
    AFFIX object TO α#(the_arm);
    END grasping;

The following is a typical call on such a routine:

	grasp(the_arm=YELLOW, object=bracket,
		grasp_point=bracket_grasp,
		special_approach=FRAME(ROT(Z,90*DEG),VECTOR(0,0,-3)),
		opening_for_approach=3*CM);

which expands into:

	MOVE YELLOW TO bracket_grasp
	    WITH DEPARTURE=NILDEPROACH
	    VIA YELLOW * FRAME(NILROT,10*Z) THEN
		BEGIN
	        OPERATE YFINGERS WITH OPENING=3*CM
		END
	    WITH APPROACH = FRAME(ROT(Z,90*DEG), VECTOR(0,0,-3));
	CENTER YELLOW
	    ON OPENING < .2*CM DO
		missed: BEGIN 
		STOP YELLOW;
		SCALAR flag;
		OPERATE YFINGERS WITH OPENING=3*CM;
		MOVE YELLOW 
		    TO YELLOW*FRAME(ROT(Z,90*DEG), VECTOR(0, 0, -3))
		    DIRECTLY;
		WRITE("Grasp failed; Type a `1' to retry");
		READ(flag);
		IF flag ≠ 1 THEN ABORT;
		MOVE YELLOW TO bracket_grasp DIRECTLY;
		CENTER YELLOW
		    ON OPENING < .2*CM DO ABORT ("Closed on air");
		END missed;
	bracket_grasp α← YELLOW;
	AFFIX bracket TO YELLOW;


.FILL
Finally, the whole task is  made into a library routine so  it can be
`called' (ie. expanded) as a subtask from a higher level task.
.NOFILL

DEFINE OZ="((72.007789*GM*CM)/(SEC*SEC))";

ROUTINE bolt_on_bracket;
    whole_task: BEGIN
    PLAN IF α#(YELLOW) ≠ YPARK
	THEN PLAN ERROR("The yellow arm is not planned to be in its
	    park position, contrary to assumption in routine bolt_on_bracket");
    PLAN IF FORM(AFFIXED, ANYTHING, YELLOW)
	THEN PLAN ERROR("Something is affixed to the yellow hand;
	    the routine bolt_on_bracket expects the hand to be empty.");
.COMT 10
	α{This type of compile-time check and warning to the
	    user is very useful for insuring that the interface
	    assumptions for routines are met in the planning
	    world just before the routine is expanded.  Notice that
	    there is a built-in procedure, PLAN ERROR, which prints
	    the included message at compile-time and stops the compilation.
	    There is also a compile-time WRITE statement, PLAN WRITE("...").
	    These two different `output' statements are used so that the
	    user can generate WRITE statements during the compilation of 
	    a program.α}
.END

    COBEGIN
	ypickup: BEGIN α{%4Pick up bracket with YELLOW%*α}
	grasp(grasp_point=bracket_grasp, object=bracket,
	    opening_for_approach=3*CM);
	MOVE bracket_hole TO beam_hole + VECTOR(0, 0, 1.3) WRT beam_hole;
	MOVE YELLOW TO ⊗ + VECTOR(0, 0, .5) WRT beam_hole
	    ON FORCE(Z WRT beam_hole) > 50*OZ DO STOP YELLOW
	    ON ARRIVAL DO ABORT("I Seem to have gone too far.");
	END ypickup;

	bpickup: BEGIN α{%4Pick up bolt with BLUE%*α}
	grasp(the_arm=BLUE, object=bolt, grasp_point=bolt,
	    opening_for_approach=3*CM)
	END bpickup
    COEND;

    MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole;
    normal_search(BLUE, .2*CM, 1.6*CM, 60*OZ, 9);
.COMT 8
α{Assume that the bolt is now in the hole.α}
.END
    MOVE BLUE TO ⊗ * FRAME(ROT(Z,90*DEG), VECTOR(0, 0, 4))
 	ON FORCE(Z WRT BLUE) > 60*OZ DO STOP BLUE;

    disengage: COBEGIN 
	foryellow: BEGIN 
	OPERATE YFINGERS WITH OPENING = 3*CM;
	UNFIX bracket FROM YELLOW;
	AFFIX bracket TO beam;
	MOVE YELLOW TO YPARK
	END foryellow;

	forblue: BEGIN 
	OPERATE BFINGERS WITH OPENING = 3*CM;
	UNFIX bolt FROM BLUE;
	AFFIX bolt TO beam;
	MOVE BLUE TO BPARK
	END forblue
    COEND disengage
END whole_task;


.NEWSS EXAMPLES OF COORDINATED ACTION, COORDINATED ACTION

.FILL
These two examples take into account some  of the more subtle aspects
of  assembly such as freeing  the bracket while trying  to insert the
bolt in the hole and changing the speed of the driver dynamically.

The following section of code is designed to simultaneously free the
YELLOW arm and move the BLUE arm to insert the bolt.  The freeing of
the YELLOW arm is to allow the bracket to accommodate slightly along the
surface of the beam as the BLUE arm tries to insert the bolt.
.NOFILL

MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole;
.COMT 4
   α{Remember that the bolt is in the BLUE hand.α}
.END
MOVE YELLOW TO ⊗
    WITH FORCE = 0 ALONG X,Y OF beam_hole
    ON DURATION > 0*SEC DO
	insertion: BEGIN 
.COMT 10
		α{Notice that "DURATION > 0*SEC" is an
		    approximation to simultaneous motion.α}
.END
	normal_search(BLUE, .2*CM, 1.6*CM, 60*OZ, 9);
.COMT 10
		   α{Assume that the bolt is now in the hole.α}
.END
	MOVE BLUE TO ⊗ * FRAME(ROT(Z,90*DEG), VECTOR(0, 0, 4))
 	    ON FORCE(Z WRT BLUE) > 60*OZ DO STOP YELLOW;
	END insertion
    ON DURATION > 4*SEC DO ABORT("Operation took too long");
.COMT 8
	α{The "ON DURATION > 4*SEC DO ABORT" will generate an error
	    if the insertion takes more than 4 seconds.  The error
	    will force the operator to deal with the situation at
	    supervisor level.α}
.END

Without the SEARCH this could be accomplished in "weak" synchrony:

MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole;
MOVE [BLUE : YELLOW]
    TO [⊗ + VECTOR(0, 0, 1.6) WRT BLUE : ⊗]
    WITH [ : FORCE = 0 ALONG X,Y OF beam_hole]
    ON [FORCE(Z WRT BLUE) > 60*OZ : ] DO [STOP : STOP];

.FILL

It is awkward to include  the SEARCH in such a scheme.  In fact, this
type  of coordination  comes up  in a  number of  other places.   For
example, if you want to operate a device (eg. the DRIVER) and move an
arm or camera "at the same time." Events and synchronizing primitives
have  been  added to  solve  these  control problems.    Consider the
following way of programming this task:

.NOFILL

EVENT y_ready, b_ready;
.COMT 4
	α{y_ready is an event signalling that the YELLOW
	    arm is ready to move.  b_ready indicates that the
	    BLUE arm is ready to move.α}
.END
MOVE bolt TO beam_hole + VECTOR(0, 0, -5.3) WRT beam_hole;
bolt_insert: COBEGIN 
    free_yellow: BEGIN 
    SIGNAL y_ready;
    WAIT b_ready;
    MOVE YELLOW TO ⊗
	WITH FORCE = 0 ALONG X,Y OF beam_hole
	ON DURATION > 4*SEC DO ABORT("Took too long");
    END free_yellow

    blue_insert: BEGIN α{%4Use blue to insert bolt%*α}
    SIGNAL b_ready;
    WAIT y_ready;
    normal_search(BLUE, .2*CM, 1.6*CM, 60*OZ, 9);
.COMT 8
	α{Assume that the bolt is now in the hole.α}
.END
    MOVE BLUE TO ⊗ * FRAME(ROT(Z,90*DEG), VECTOR(0, 0, 4))
	ON FORCE(Z WRT BLUE) > 60*OZ DO STOP YELLOW;
    END blue_insert;
COEND bolt_insert;

.FILL

Consider the problem of inserting in a screw and checking to make sure
that it  does not bind.   If, after a short time,  the screw does not
bind, the speed of the DRIVER can be increased.  However, if  it DOES
bind, everything should stop and the DRIVER should be reversed to try
to unbind the screw.

.NOFILL

EVENT d_ready, b_ready;
SCALAR sp, flag;
sp α← 30;
flag α← 1;
WHILE flag DO
    screw_loop: BEGIN 
    move_screw: COBEGIN α{%4Move and screw simultaneously%*α}
	drive: BEGIN 
	SIGNAL d_ready;
	WAIT b_ready;
	OPERATE DRIVER
	    WITH VELOCITY = sp
	    ON DURATION > 8*SEC DO ABORT("Took too long");
	END drive

	downward_force: BEGIN 
	SIGNAL b_ready;
	WAIT d_ready;
	MOVE BLUE TO ⊗
	    WITH FORCE = 0 ALONG Z OF BLUE
	    WITH FORCE = 40*OZ ALONG Z OF BLUE
	    bind: ON TORQUE(Z WRT BLUE) > 80*OZ DO
		bound: BEGIN 
		DISABLE catch_ok;
		STOP BLUE;
		STOP DRIVER;
		COBEGIN α{%4Try to unbind by reversing the driver%*α}
		    unscrew: BEGIN 
		    SIGNAL d_ready;
		    WAIT b_ready;
		    sp α← -60;
		    OPERATE DRIVER
			WITH VELOCITY = SP
			ON DURATION > 4*SEC DO ABORT("Can't unbind");
		    END unscrew

		    upward_force BEGIN
		    SIGNAL b_ready;
		    WAIT d_ready;
		    MOVE BLUE TO ⊗
			WITH FORCE = 0 ALONG Y, X OF BLUE
			WITH FORCE = 40*OZ ALONG Z OF BLUE
			out_ok: ON FORCE(Z WRT BLUE)<20*OZ DO
			    BEGIN
			    STOP DRIVER;
			    STOP BLUE;
.COMT 22
			α{Leave flag true for retry.α}
.END
			    END
			too_much_time: ON DURATION>4*SEC DO ABORT;
		    END upward_force
	        COEND 
		END bound
	    catch_ok: ON DURATION > 1*SEC DO
		BEGIN
 		DISABLE bind;
		ENABLE torqued_in_ok;
		sp α← 60; α{maybe this should be CRITICAL.α}
		END
 	    torqued_in_ok: DEFER ON TORQUE(Z WRT BLUE) > 80*OZ DO
		BEGIN
		STOP DRIVER;
		STOP BLUE;
		flag α← 0; α{indicating no retryα}
		END;
  	END downward_force
    COEND move_screw
    END screw_loop;

.NEWSS A α`VERY HIGH LEVELα' EXAMPLE

.FILL
This very short example demonstrates the use of assembly-oriented
special primitives to simplify a task specification, as well as
some of the object description conventions used by those primitives.
Here, the task is the same as that of {sssref ex1}.  For a fuller
explanation of the use of such primitives and another, longer
example, see {secref vhl}.
.NOFILL

FRAME beam, bracket, bolt;
FRAME bracket_bore, beam_bore;
FRAME bolt_grasp, bracket_handle;

.comt 0
α{We must first describe the various components.  We expect that
eventually the process of making such descriptions will become
very largely automated, as computer programs begin to play an
increasingly active role in mechanical design. See {ssref obd}.α}
.end

ASSERT FORM(TYPE, beam, object);
ASSERT FORM(GEOMED, beam, "beam.B3D[AL,HE]"); α{%4Shape description%*α}
ASSERT FORM(SUBPART, beam, beam_bore);
ATTACH beam_bore TO beam RIGIDLY AT TRANS(ROT(Y,90),VECTOR(0,1.5,6));

ASSERT FORM(TYPE, bracket, object);
ASSERT FORM(GEOMED, bracket, "BRACK.B3D[AL,HE]"); α{%4Shape description.%*α}
ASSERT FORM(SUBPART, bracket, bracket_bore);
ASSERT FORM(SUBPART, bracket, bracket_handle);
ATTACH bracket_bore TO bracket RIGIDLY AT TRANS(ROT(X,180),VECTOR(5.1,2,0));
ATTACH bracket_handle TO bracket RIGIDLY AT TRANS(ROT(X,180),NILVEC);

ASSERT FORM(TYPE, bolt, SHAFT);
ASSERT FORM(DIAMETER, bolt, 0.5*CM);
ASSERT FORM(TOP_END, bolt, head_type1);
ASSERT FORM(BOTTOM_END, bolt, tiptype1);
ASSERT FORM(TYPE, tiptype1, FLAT_END);

ASSERT FORM(TYPE, bracket_bore, BORE);
ASSERT FORM(DIAMETER, bracket_bore, 0.502*CM);
ASSERT FORM(LENGTH, bracket_bore, 0.5*CM);
ASSERT FORM(TOP_END, bracket_bore, bracket_hole1);
ASSERT FORM(BOTTOM_END, bracket_bore, bracket_hole1);

.comt 4
	α{Et ceteraα}
.end

.comt 0
α{Also, describe how things go together:α}
.end
ASSERT FORM(TYPE, beam_assembly, ASSEMBLY);
ASSERT FORM(SUBPART, beam_assembly, beam);
ASSERT FORM(SUBPART, beam_assembly, bolt);
ASSERT FORM(SUBPART, beam_assembly, bracket);

ASSERT FORM(bracket, FITS_ONTO, beam_assembly, AT, 
    TRANS(ROT(Y,90),VECTOR(5.1,2,0));
ASSERT FORM(bolt, FITS_ONTO, beam_assembly, AT, 
    TRANS(ROT(Y,90),VECTOR(5.1,2.3,0));

ASSERT FORM(MATED, beam_hsurf, bracket_bottom);
ASSERT FORM(ALIGNED, beam_bore, bracket_bore);
ASSERT FORM(RUNS_THRU, bolt, bracket_bore);
ASSERT FORM(RUNS_THRU, bolt, beam_bore);
.COMT 4
	α{Et cetera.α}
.END

.comt 0
α{Now, describe the initial scene.  Here, assume that the initial object
locations are known precisely.α}
.end
bracket α← FRAME(NILROT,VECTOR(20,40,0));
beam α← FRAME(NILROT,VECTOR(10,60,0));
bolt α← FRAME(ROT(Y,180),VECTOR(30,50,5));

grasp bracket AT TRANS(ROT(Y,180),2*Z) WITH YELLOW;
.comt 0
α{The system will use its internal model of the bracket to
fill in the expected hand opening.α}
.end

FIT bracket ONTO beam_assembly
	USING YELLOW
	AFTERWARDS HOLD bracket WITH YELLOW;

.comt 0
α{The system will use the object description information to fill
in the exact location to which to move the bracket.  Also, it
will pick appropriate techniques to ensure that the bracket
is appropriately aligned.  The AFTERWARDS clause tells the
system that it is to use the yellow arm to hold the bracket in placeα}
.end

INSERT bolt INTO bracket_hole1 USING BLUE;

.comt 0
α{Once again, the system will fill in the details, such as how
the bolt is to be grasped, how it should be brought 
to the hole, how it will be pushed in, and so forth.α}
.end

RELEASE bracket; α{%4Since the bolt now holds it on.%*α}


.FILL